/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file x.h
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#ifndef JOGCONTROL_H
#define JOGCONTROL_H
#include "trajectoryudefine.h"
#include "MacroDefine/GeneralDefine.h"
#include "trajectoryserver.h"
#define JOG_SEGMENT_LENGTH_ANGULAR 3600 //手动规划段长，角度
#define JOG_SEGMENT_LENGTH_LINE 10000 //手动规划段长，mm单位
#define JOG_FIND_MAX_COUNT 20
#define JOG_FIND_QULIFIY_DELTA_DISTANCE 0.01  //UNIT M米

#define JOG_ROTATION_ANGULAR M_PI/1.5 //手动姿态旋转角度

/**
 * @brief The JogControl class 机械臂的手动逻辑控制
 */
class JogControl
{
public:
    JogControl(TrajectoryServer *trajServerIn, RobotCommonParameter robotParameterIn, MotionMessage* motionMessageIn);

public:
    int setRobotCoordinateConfig(struct RobotCoordinateAxisConfig coordinateConfigIn);
    int allowSingleAxisJog(bool enable);
    int jog(int axis, int direction,int velocityOpt);
    int jogForJoint(int axis, int direction,int velocityOpt);
    int jogForToolCoordinateTranslate(int axis, int direction,int velocityOpt);
    int jogForToolCoordinateRotation(int axis, int direction,int velocityOpt);
    int jogForUserCoordinateTranslate(int axis, int direction,int velocityOpt);
    int jogForUserCoordinateRotation(int axis, int direction,int velocityOpt);
    int jogForWorldCoordinateTranslate(int axis, int direction,int velocityOpt);
    int jogForWorldCoordinateRotation(int axis, int direction,int velocityOpt);
    int getJogCoordinate();
    int setJogCoordinate(int coordinateType);

    int returnZeroPoint(int axisNumber, int &returnFlag);//-1全部轴回零
    int moveToPoint(const MoveParameter moveParameter);//TODO  支持码垛点 //hualei todo

 private:
    int getTargetPoint(int axis, int direction, int coordinateType, PointPro &targetPoint);
    void addMsg(int messageLevel,
                std::string componentName,
                std::string messageType,
                int messageCode,
                int robotIdIn,
                int parameter1 = 0,
                int parameter2 = 0,
                int parameter3 = 0,
                int parameter4 = 0);
 private:
        int robotId;
        volatile int currentJogCoordinate; // 0 关节    1 用户    2 工具    3 世界
        int jogAxis;//手动运动轴号，从0到关节数量jointNum
        int jogDirection;//寸动方向
        bool isJogRun;//是否寸动
        int jointNum;
        int internalNum;
        int externalNum;
        TrajectoryServer *trajServer;
        RobotCommonParameter robotParameter;
        MotionMessage *motionMessage;
        bool allowSingleAxisJogFlag;

};

#endif // JOGCONTROL_H
